#include "track.h"

#include "device.h"
#include "fuzzy_control.h"

/* 前进循迹 */
void track_drive(track_speed_t s)
{
  motor_move(s, 30*gray_front_error());
}

/* 倒车循迹，前后灰度err定义相同，差速值加个负号，逻辑相反 */
void track_reverse(track_speed_t s)
{
  motor_move(-s, -30*gray_back_error());
}

void track_stop(void)
{
  motor_move(0, 0);
}

/* 停在十字路口时 左转 */
void track_turn_left(track_speed_t s)
{
  motor_move(0, -s);

  delay_ms(500);

  while (gray_turn_not_ok0());  /* 没转到位，程序停在此处 */

  motor_move(0, 0);
}

/* 纯延时，开环转 */
void track_turn_left_time(track_speed_t s, int ms)
{
  motor_move(0, -s);

  delay_ms(ms);

  motor_move(0, 0);
}

void track_turn_right(track_speed_t s)
{
  motor_move(0, s);
  
  delay_ms(500);  

  while (gray_turn_not_ok0());  /* 没转到位，程序停在此处 */

  motor_move(0, 0);
}


/* 循迹测试函数 */
void track_drive_test(void)
{
  while (1)
  {
    /* 20ms执行一次控制 */
    delay_ms(20);

    /* 车前进速度和差速值控制，error前的系数调整转弯力度 */
    motor_move(300, 300 * gray_front_error());
//    motor_move(300, fuzzy_velocity_difference());


    /* 遇到十字路口，停车，跳出控制循环 */
    if (gray_is_cross())
    {
      motor_move(0, 0);
      break;
    }
  }

  while (1);   /* 测试程序停在此处 */
}


void track_reverse_test(void)
{
  while (1)
  {
    delay_ms(20);

    track_reverse(speed_medium);

    /* 遇到十字路口，停车，跳出控制循环 */
    if (gray_is_cross())
    {
      motor_move(0, 0);
      break;
    }
  }

  while (1);   /* 测试程序停在此处 */
}


void track_turn_test(void)
{
  while (1)
  {
    /* 20ms执行一次控制 */
    delay_ms(20);

    /* 车前进速度和差速值控制，error前的系数调整转弯力度 */
    motor_move(300, 260 * gray_front_error());

    /* 遇到十字路口，停车，跳出控制循环 */
    if (gray_is_cross())
    {
      motor_move(0, 0);
      break;
    }
  }

  delay_ms(500);

  track_turn_left(speed_medium);

  while (1);
}




//void track_ps2_control_test(void)
//{
//  while(1)
//  {
//    if (ps2_is_red_mode())
//    {
//      ps2_data_refresh();
//      
//      printf("%d %d %d %d \n", ps2_data.lx, ps2_data.ly, ps2_data.rx, ps2_data.ry);
//    
//      printf("l1: %d\n", ps2_data.l1);
////      printf("l2: %d\n", ps2_data.l2);
////      printf("l3: %d\n", ps2_data.l3);
////      printf("r1: %d\n", ps2_data.r1);
////      printf("r2: %d\n", ps2_data.r2);
////      printf("r3: %d\n", ps2_data.r3);

////      motor_move(ps2_data.ly*2, ps2_data.lx*2);   
//    }
//    delay_ms(800);
//  }

//}
